#ifndef MATRIX4_HEADER_VIZMF_PROJECT
#define MATRIX4_HEADER_VIZMF_PROJECT

#include "src/core/generic/vector3.h"

class Matrix4
{
public:
  typedef double float_type;
  enum StorageType {RowMajor, ColMajor};

  Matrix4() {}
  Matrix4(float_type *data, StorageType type = RowMajor)
  {
    switch (type)
    {
    case RowMajor:
      for (int i = 0; i < 16; ++i) data_[i] = data[i];
      break;
    case ColMajor:
      for (int i = 0 ; i < 4; ++i) for (int j = 0; j < 4; ++j) data_[i*4 +j] = data[j*4+i];
      break;
    }
  }
  Matrix4(float_type **data, StorageType type = RowMajor)
  {
    switch (type)
    {
    case RowMajor:
      for (int i = 0 ; i < 4; ++i) for (int j = 0; j < 4; ++j) data_[i*4 + j] = data[i][j];
      break;
    case ColMajor:
      for (int i = 0 ; i < 4; ++i) for (int j = 0; j < 4; ++j) data_[i*4 + j] = data[j][i];
      break;
    }
  }
  void loadIdentity() { for (int i = 0 ; i < 4; ++i) for (int j = 0; j < 4; ++j) data_[i*4 +j] = (i == j) ? 1 : 0; }
  void loadZero() { for (int i = 0; i < 16; ++i) data_[i] = 0.0; }
  const float_type* operator [](int i) const {return &(data_[i*4]); }
  float_type* operator [](int i) {return &(data_[i*4]); }
  Vector3 operator * (const Vector3& vec) const
  {
    return Vector3(data_[0] * vec[0] + data_[1] * vec[1] + data_[2] * vec[2] + data_[3],
                   data_[4] * vec[0] + data_[5] * vec[1] + data_[6] * vec[2] + data_[7],
                   data_[8] * vec[0] + data_[9] * vec[1] + data_[10] * vec[2] + data_[11]);
  }
  Matrix4 operator * (const Matrix4& mat) const
  {
    Matrix4 a;
    a.loadZero();
    for (int i = 0 ; i < 4; ++i) for (int j = 0 ; j < 4; ++j)
      for (int k = 0 ; k < 4; ++k)
      {
        a[i][j] += data_[i*4 + k] * mat.data_[k*4 + j];
      }
    return a;
  }
  const Matrix4& operator *= (const Matrix4& mat)
  {
     return *this = mat * *this;
  }

  // some comvenience functionce follow
  void rotate(const Vector3& axis = Vector3(1.0, 0.0, 0.0), const float_type angle = 0.0)
  {
    Matrix4 rot;
    Vector3 a = axis; a.normalize();
    Vector3 b = Vector3(a[0]*a[0], a[1]*a[1], a[2]*a[2]);
    float_type c = cos(angle), s = sin(angle);
    rot[0][0] = b[0] + (1.0 - b[0]) * c;
    rot[0][1] = a[0]*a[1] * (1.0 - c) - a[2]*s;
    rot[0][2] = a[0]*a[2] * (1.0 - c) + a[1]*s;
    rot[0][3] = 0.0;
    rot[1][0] = a[0]*a[1] * (1.0 - c) + a[2]*s;
    rot[1][1] = b[1] + (1.0 - b[1])*c;
    rot[1][2] = a[1]*a[2]*(1.0 - c) - a[0]*s;
    rot[1][3] = 0.0;
    rot[2][0] = a[0]*a[2]*(1.0 - c) - a[1] * s;
    rot[2][1] = a[1]*a[2]*(1.0 - c) + a[0] * s;
    rot[2][2] = b[2] + (1.0 - b[2])*c;
    rot[2][3] = 0.0;
    rot[3][0] = 0.0;
    rot[3][1] = 0.0;
    rot[3][2] = 0.0;
    rot[3][3] = 1.0;
    *this *= rot ;
  }
  void scale(const Vector3& vec)
  {
    Matrix4 sc;
    sc[0][0] = vec[0];
    sc[0][1] = 0;
    sc[0][2] = 0;
    sc[0][3] = 0;
    sc[1][0] = 0;
    sc[1][1] = vec[1];
    sc[1][2] = 0;
    sc[1][3] = 0;
    sc[2][0] = 0;
    sc[2][1] = 0;
    sc[2][2] = vec[2];
    sc[2][3] = 0;
    sc[3][0] = 0;
    sc[3][1] = 0;
    sc[3][2] = 0;
    sc[3][3] = 1;
    *this *= sc;
  }

  void translate(const Vector3& vec)
  {
    Matrix4 mat;
    mat.loadIdentity();
    mat[0][3] = vec[0];
    mat[1][3] = vec[1];
    mat[2][3] = vec[2];
    *this *= mat;
  }

private:
  float_type data_[16];
};


inline Vector3 operator * (const Vector3& vec, const Matrix4& mat)
{
  return Vector3(mat[0][0] * vec[0] + mat[0][1] * vec[1] + mat[0][2] * vec[2] + mat[0][3],
                 mat[1][0] * vec[0] + mat[1][1] * vec[1] + mat[1][2] * vec[2] + mat[1][3],
                 mat[2][0] * vec[0] + mat[2][1] * vec[1] + mat[2][2] * vec[2] + mat[2][3]);
}


inline std::ostream& operator << (std::ostream& strm, const Matrix4& mat)
{
  return strm << mat[0][0] << " " << mat[0][1] << " " << mat[0][2] << " " << mat[0][3] << std::endl
       << mat[1][0] << " " << mat[1][1] << " " << mat[1][2] << " " << mat[1][3] << std::endl
       << mat[2][0] << " " << mat[2][1] << " " << mat[2][2] << " " << mat[2][3] << std::endl
       << mat[3][0] << " " << mat[3][1] << " " << mat[3][2] << " " << mat[3][3] << std::endl;

}


#endif // MATRIX4_HEADER_VIZMF_PROJECT
